/*
 * Controller.cpp
 *
 *  Created on: 22 янв. 2013
 *      Author: alex
 */

#include "Controller.h"
#include <cmath>
#include <cstdio>

Controller* Controller::_instance=0;

Controller::~Controller()
{

}
Controller::Controller():position(0,0),_odometry(0),_start(0,0)
{


}

Controller* Controller::instance()
{
	//static Controller* _instance;
	if(_instance == 0)
		_instance = new Controller();
	return _instance;
}
void Controller::setOdometryModule(IOdometry* odometry)
{
	_odometry = odometry;



}


void Controller::setTile(const Tile& tile)
{
	position = OccupancyGrid::instance()->LogicalToReal(tile);
}


void Controller::setAngle(double Angle)
{
	position.angle = Angle;
}

void Controller::setStart(const Tile& tile)
{
	_start = tile;
}

void Controller::goTo(const Tile& end)
{
	Tile curr = getTile();
	int goX = end.x - curr.x;
	int goY = end.y - curr.y;
	Tile mid(0,0);
	if(goX>goY)
	{
		mid.x = end.x;
		mid.y = curr.y;
	}
	else
	{
		mid.x = curr.x;
		mid.y = end.y;
	}



	Point p1=OccupancyGrid::instance()->LogicalToReal(mid),p2 = OccupancyGrid::instance()->LogicalToReal(end);
	if(mid != getTile())
	{
		printf("point 1: %lf %lf\n",p1.x,p1.y);
		goToStraight(p1);
	}
	if(p1 != p2)
	{
		printf("point 2: %lf %lf\n",p2.x,p2.y);
		goToStraight(p2);
	}

}

void Controller::goToStraight(Point& dest)
{

	double dx = dest.x - position.x;
	double dy = dest.y - position.y;
	printf("goToStraight: dx=%lf dy=%lf\n",dx,dy);

	if(fabs(dx) > fabs(dy))//horisontal movement
	{
		if(dx > 0)//to the right
		{
			dest.angle = 0;

		}
		else//to the left
		{
			dest.angle = M_PI;
		}

	}
	else//vertical
	{
		if(dy > 0)//down
		{
			dest.angle = M_PI_2;

		}
		else//to the left
		{
			dest.angle = -M_PI_2;
		}

		//position = _odometry->turnTo(dest.angle);
	}
//	double alpha = dest.angle - position.angle;
//	while (dest.angle > M_PI)
//			alpha -= M_2_PI;
	printf("turnTo : %lf\n", dest.angle);
	position = _odometry->turnTo(dest.angle);
	position = _odometry->goTo(dest);


}


void Controller::goHome()
{
	goTo(_start);

}

